import java.io.BufferedReader;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileReader;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Vector;


public class CoordinatedFormation extends Skills {

	enum TEST_MODE{
		Leader_run_Follower_idel,
		Leader_idle_Follower_run,
		Leader_run_Follower_run,
		Leader_idle_Follower_idle
	} 
	TEST_MODE test_mode = TEST_MODE.Leader_run_Follower_run;
	ForagingMission_TRAJECTORY trajectory = ForagingMission_TRAJECTORY.LINEAR_WITH_STOP;
	FOLLOW_TYPE follow_type = FOLLOW_TYPE.follow_anchor_localization;
	
	protected void setupBehaviors() {
		

		add("Perception", 
				new BT.Par("Perception")
					.add(new TrackNeighbors())
					.add(new TrackGoalPosition())
		);
		
		add("ConflictDetection", new SomebodyLostOrFar(BT.LOOP.UNTIL));

		add("Allocation", new LeaderAllocation());
		switch(test_mode){
		case Leader_idle_Follower_idle:
			add("Leader", new Say("I am Leader"));
			add("Follower", new Say("I am Follower"));
			break;
		case Leader_run_Follower_idel:
			add("Leader", new ForagingMission(trajectory));
			add("Follower", new Say("I am Follower"));
			break;
		case Leader_idle_Follower_run:
			add("Leader", new Say("I am Leader"));
			add("Follower", new Follow(follow_type));
			break;
		case Leader_run_Follower_run:
			add("Leader", new ForagingMission(trajectory));
			add("Follower", new Follow(follow_type));
			break;
		}
		add(new Stop());

		add(new StartConflict());
		add(new EndConflict());
		add("Solution", 
			new BT.Swi("Solution")
				.add(new SelectConflictSolution())
				
				.add(new TimedRandomMove())
				.add(new RandomMoveUntilSolved())
				.add(new ChangeLeader())
				.add(new ChangeLeaderAndValidateLocation())
		);		
		
		loadTaskFromFile("Mission", "formation");
			
		behavior=
		loadTaskFromFile("Start", "Coordination");

	}
	
	protected void onTrialInit() {
		super.onTrialInit();
		setGoals();		
	}
	
	String[] readLine(BufferedReader r){
		String line;
		try {
			line = r.readLine();
		} catch (IOException e) {
			line = null;
			e.printStackTrace();
		}
		if(line==null) return null;
		String[] t = line.split(" ");
		Vector<String> tt = new Vector<String>();
		for(String s: t)if(s.trim().length()==0)continue;else{
			if(s.startsWith("//")) break;
			tt.add(s.trim());
		}
		String[] res = new String[tt.size()];
		return tt.toArray(res);
	}
	private String robotDescription = null; 
	Vec2D getPositionOfRobot(int rid){
		File file = new File( "formation.dsc" );
		try {
			BufferedReader fr = new BufferedReader(new FileReader(file));
			int robot_counter=-1;
			int args = 0;
			boolean found = false;
			ArrayList<String> robot = new ArrayList<String>();
			String[] line = readLine(fr);
			while(line != null){
				if(line.length>0){
					int args_added=0;
					if(line[0].equals("robot")){
						robot_counter++;
						if(robot_counter==rid){
							for(String s: line){ robot.add(s); args_added++; }
						}
					}
					if(args>0){
						for(String s: line){ robot.add(s); args_added++; }
					}
					args+=args_added;
					if(args>=9){
						found = true;
						break;
					}
				}
				line = readLine(fr);
			}
			String rr = ""; for(String s: robot) rr+=s+" ";
			if(found){
				robotDescription = rr;
				double x = Double.parseDouble( robot.get(3) );
				double y = Double.parseDouble( robot.get(4) );
				return Vec2D.cortezian(x, y);
			}
		} catch (FileNotFoundException e) {
			e.printStackTrace();
		}

		return null;
	}

	void setGoals(){
		wm.anchorId = wm.getMyId()-1;//wm.team.getLeaderId();

		Vec2D leaderPos = getPositionOfRobot(wm.team.getLeaderId());
		if(leaderPos==null)
			print("Robot description in file not found: leader("+wm.team.getLeaderId()+")");
		else
			print("Robot description of leader("+wm.team.getLeaderId()+"): "+robotDescription);
		
		Vec2D myPos = getPositionOfRobot(wm.getMyId());
		if(myPos==null)
			print("Robot description in file not found: me("+wm.getMyId()+")");
		else
			print("Robot description of me("+wm.getMyId()+"): "+robotDescription);
		wm.setGoalPosition(leaderPos.sub(myPos));
	}


}
